/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "OccuGUI.h"


COccuGUI::COccuGUI(const char *mapfile,float resolution, float scale_){
	screen = NULL;
	grid = new TOccupancyGrid(1, 1, resolution, Grid::TCoord2D(0,0));
	grid->loadLineMap(mapfile, 1.0,1.0);
	scale = scale_;
}

COccuGUI::COccuGUI(float size,float resolution, float scale_){
		screen = NULL;
		grid = new TOccupancyGrid(size, size, resolution, Grid::TCoord2D(0,0));
		scale = scale_;
}

void COccuGUI::setOccupancyGrid(	TOccupancyGrid *gr){
		int Nx,Ny;
		float *to_data,*from_data;
		grid->getDim(Nx,Ny);
		to_data = grid->getDataPtr();
		from_data = gr->getDataPtr();
		for(int i=0;i<Nx*Ny;i++){
				to_data[i] = from_data[i];
		}
}
/**
* Plots the map using scaling
**/
void COccuGUI::plotMap(){
		int Nx,Ny;
		grid->getDim(Nx,Ny);
		float *data;
		data = grid->getDataPtr();
		if( fabs(scale-1.0)<0.01){
				for(int i=0;i<Nx*Ny;i++){
						pixelRGBA(screen, i%Nx, i/Nx, 0, 0, 255*data[i],255);
				}
		}else{
				//fprintf(stderr,"here (%d,%d)\n",Nx,Ny);
			for(int i=0;i<Nx;i++){
				for(int j=0;j<Ny;j++){
						float pix = scale*((float)i);
						float piy = scale*((float)j);
						//fprintf(stderr,"(%.2f %.2f) ",pix,piy);
						pixelRGBA(screen, (int)(pix), (int)(piy), 0, 0, 255*data[Nx*j+i],255);
				}
			}
		}					
}

void COccuGUI::screenToMapPixel(Grid::TPixel &pix){
		pix.x = (int) ((float)pix.x)/scale; 
		pix.y = (int) ((float)pix.y)/scale;
}

void COccuGUI::mapToScreenPixel(Grid::TPixel &pix){
		pix.x = (int) ((float)pix.x)*scale; 
		pix.y = (int) ((float)pix.y)*scale;
}

///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
/// Draw the grid 
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////

bool COccuGUI::drawGrid(){
	SDL_Event event;
	bool run = true;	
	initSDL();
	int Nx,Ny;
	grid->getDim(Nx,Ny);
	float *data;
	data = grid->getDataPtr();
	int cnt = 0;
	std::vector<Grid::TPixel> pixels;
 	bool out=true; 
	while(run){
		// Wait for incoming event
		while (SDL_PollEvent(&event)) {
			// Switch based on event type.
			switch (event.type) {
				case SDL_QUIT: {run = false;break;}
				case SDL_KEYDOWN: {
					switch (event.key.keysym.sym) {
						case SDLK_UP: break;
						case SDLK_DOWN:break;
						case SDLK_q:{run = false;out = false;break;}
						case SDLK_ESCAPE:{run = false;  break;}
						case SDLK_BACKSPACE:{if(pixels.size()>0)pixels.pop_back(); cnt=0;break;}
						case SDLK_s:{ save(); break;}
						default:break;
					}///<switch (event.key.keysym.sym)
				break;
				}///<case SDL_KEYDOWN:
				case SDL_MOUSEBUTTONDOWN:{
					Grid::TPixel pix(event.button.x, event.button.y);
					screenToMapPixel(pix);
					Grid::TCoord2D coord = grid->toCoord(pix);
					
					printf("Mouse button %d pressed at (%d,%d) = (%.3fm,%.3fm) \n",
								 event.button.button, event.button.x, event.button.y,coord.x,coord.y);
					pixels.push_back(Grid::TPixel(event.button.x, event.button.y));
				}
					
				default:break;
	
			}///<switch (event.type)
		}///while SDL_PollEvent(&event)
	
		if(cnt<2){
			cnt++;
			plotMap();
			printGeneralInfo();
		}
		if(pixels.size()>1){
			for(int i=0;i<pixels.size()-1;i++){
				lineRGBA(screen, pixels[i].x, pixels[i].y,pixels[i+1].x, pixels[i+1].y, 0, 255, 0,150);
			}
		}
		
		SDL_Flip(screen);
		usleep(1000*10);
		//fprintf(stderr,"Running\n");
	}///<While(run)
	
	cleanSDL();
	SDL_Quit();
	return out;
}

///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
/// GET 2D Environment Measurement
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////

bool COccuGUI::generateMeasurement(CEnvironmentMeasurement2D &meas){
		SDL_Event event;
		bool run = true;	
		initSDL();
		int Nx,Ny;
		grid->getDim(Nx,Ny);
		float *data;
		data = grid->getDataPtr();
		int cnt = 0;
		std::vector<Grid::TPixel> pixels;
		std::vector<Grid::TCoord2D> coords;
		TMeasurementPoint mPoint;
		
		meas.points.clear();
		bool out = true;
		while(run){
		// Wait for incoming event
				while (SDL_PollEvent(&event)) {
			// Switch based on event type.
						switch (event.type) {
								case SDL_QUIT: {run = false;break;}
								case SDL_KEYDOWN: {
										switch (event.key.keysym.sym) {
												case SDLK_UP: break;
												case SDLK_DOWN:break;
												case SDLK_q:{run = false;out = false;break;}
												case SDLK_ESCAPE:{run = false; break;}
												case SDLK_BACKSPACE:{if(pixels.size()>0)pixels.pop_back(); cnt=0;break;}
												case SDLK_s:{ save(); break;}
												default:break;
										}///<switch (event.key.keysym.sym)
										break;
								}///<case SDL_KEYDOWN:
								case SDL_MOUSEBUTTONDOWN:{
										Grid::TPixel pix(event.button.x, event.button.y);
										screenToMapPixel(pix);
										Grid::TCoord2D coord = grid->toCoord(pix);
				
										printf("Mouse button %d pressed at (%d,%d) = (%.3fm,%.3fm) \n",
													 event.button.button, event.button.x, event.button.y,coord.x,coord.y);
										
										coords.push_back(coord);
										pixels.push_back(Grid::TPixel(event.button.x, event.button.y));
										float phii;
										if(coords.size()==1){ 
												phii=0;
												meas.frame.setFrame(coord.x,coord.y,phii); ///< Initial frame
										}
										else{
												phii = atan2( (coords[coords.size()-1].y-coords[coords.size()-2].y),
																(coords[coords.size()-1].x-coords[coords.size()-2].x));
										}
										Grid::TScan s; 
										s.alloc(181);
										
										grid->getScan(coord.x,coord.y,phii-M_PI/2.0,181,M_PI/180.0,0.9,s);
										
										for(int i=0;i<s.N;i++){
												TMeasurementPoint p;
												p.x = s.r[i] * cos(s.a[i]+phii-M_PI/2.0) + (coord.x-coords[0].x);
												p.y = s.r[i] * sin(s.a[i]+phii-M_PI/2.0) + (coord.y-coords[0].y);
												meas.addPoint(p);
										}
								}
					
								default:break;
	
						}///<switch (event.type)
				}///while SDL_PollEvent(&event)
	
				if(cnt<2){
						cnt++;
						plotMap();
						printGeneralInfo();
				}
				if(pixels.size()>1){
						for(int i=0;i<pixels.size()-1;i++){
								lineRGBA(screen, pixels[i].x, pixels[i].y,pixels[i+1].x, pixels[i+1].y, 0, 255, 0,150);
						}
				}
				if(meas.points.size()>0){
						for(unsigned int i=0;i<meas.points.size();i++){
								Grid::TCoord2D coord(meas.points[i].x,meas.points[i].y);
								coord.x += coords[0].x; coord.y+=coords[0].y;
								Grid::TPixel pix = grid->toPixel(coord);
								mapToScreenPixel(pix);
								pixelRGBA(screen, pix.x, pix.y, 255, 0, 0,255);
						}
				}
		
				SDL_Flip(screen);
				usleep(1000*10);
		//fprintf(stderr,"Running\n");
		}///<While(run)
	
		cleanSDL();
		SDL_Quit();
		return out;
}

///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
/// GET 2D Environment Measurement Using Occupancy grid
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////

bool COccuGUI::generateOccuMeasurement(CEnvironmentMeasurement2D &meas,float local_occu_size){
		SDL_Event event;
		bool run = true;	
		initSDL();
		int Nx,Ny;
		grid->getDim(Nx,Ny);
		float *data;
		data = grid->getDataPtr();
		int cnt = 0;
		std::vector<Grid::TPixel> pixels;
		std::vector<Grid::TCoord2D> coords;
		TMeasurementPoint mPoint;
		bool out = true;
		meas.points.clear();
		TOccupancyGrid lg(local_occu_size, local_occu_size, grid->getResolution(), Grid::TCoord2D(0,0));
		lg.setGridToValue(0.5);
		while(run){
		// Wait for incoming event
				while (SDL_PollEvent(&event)) {
			// Switch based on event type.
						switch (event.type) {
								case SDL_QUIT: {run = false;break;}
								case SDL_KEYDOWN: {
										switch (event.key.keysym.sym) {
												case SDLK_UP: break;
												case SDLK_DOWN:break;
												case SDLK_q:{run = false;out=false;break;}
												case SDLK_ESCAPE:{run = false; break;}
												case SDLK_BACKSPACE:{if(pixels.size()>0)pixels.pop_back(); cnt=0;break;}
												case SDLK_s:{ save(); break;}
												default:break;
										}///<switch (event.key.keysym.sym)
										break;
								}///<case SDL_KEYDOWN:
								case SDL_MOUSEBUTTONDOWN:{
										Grid::TPixel pix(event.button.x, event.button.y);
										screenToMapPixel(pix);
										Grid::TCoord2D coord = grid->toCoord(pix);
										printf("Mouse button %d pressed at (%d,%d) = (%.3fm,%.3fm) \n",
													 event.button.button, event.button.x, event.button.y,coord.x,coord.y);
										
										coords.push_back(coord);
										pixels.push_back(Grid::TPixel(event.button.x, event.button.y));
										float phii;
										if(coords.size()==1){ 
												phii=0;
												meas.frame.setFrame(coord.x,coord.y,phii); ///< Initial frame
										}
										else{
												phii = atan2( (coords[coords.size()-1].y-coords[coords.size()-2].y),
																			 (coords[coords.size()-1].x-coords[coords.size()-2].x));
										}
										Grid::TScan s; 
										s.alloc(181);
										
										grid->getScan(coord.x,coord.y,phii-M_PI/2.0,181,M_PI/180.0,0.9,s);
										for(int i=0;i<s.N;i++){
												s.a[i]-=M_PI/2.0;
										}
										
										lg.setScanFillProb(coord.x-coords[0].x, coord.y-coords[0].y ,phii, s,0.05,0.95);
										
								}
					
								default:break;
	
						}///<switch (event.type)
				}///while SDL_PollEvent(&event)
	
				if(cnt<2){
						cnt++;
						plotMap();
						printGeneralInfo();
				}
				if(pixels.size()>1){
						for(int i=0;i<pixels.size()-1;i++){
								lineRGBA(screen, pixels[i].x, pixels[i].y,pixels[i+1].x, pixels[i+1].y, 0, 255, 0,150);
						}
				}
				
				meas.points.clear();
				meas.setOccupancyGrid(&lg, 0.8);
				for(unsigned int i=0;i<meas.points.size();i++){
						Grid::TCoord2D coord(meas.points[i].x,meas.points[i].y);
						coord.x += coords[0].x; coord.y+=coords[0].y;
						Grid::TPixel pix = grid->toPixel(coord);
						mapToScreenPixel(pix);
						pixelRGBA(screen, pix.x, pix.y, 255, 0, 0,255);
				}
				
		
				SDL_Flip(screen);
				usleep(1000*10);
		//fprintf(stderr,"Running\n");
		}///<While(run)
		meas.setOccupancyGrid(&lg, 0.8);
		cleanSDL();
		SDL_Quit();
		return  out;
}

///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
/// Plot result
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////

bool COccuGUI::plotMatchResults(CMatchStorage &result, float x, float y, float a,float variance){
		SDL_Event event;
		bool run = true;	
		initSDL();
		int Nx,Ny;
		grid->getDim(Nx,Ny);
		float *data;
		data = grid->getDataPtr();
		int cnt = 0;
		bool out = true;
		while(run){
		// Wait for incoming event
				while (SDL_PollEvent(&event)) {
			// Switch based on event type.
						switch (event.type) {
								case SDL_QUIT: {run = false;break;}
								case SDL_KEYDOWN: {
										switch (event.key.keysym.sym) {
												case SDLK_UP: break;
												case SDLK_DOWN:break;
												case SDLK_q:{run = false; out = false; break;}
												case SDLK_ESCAPE:{run = false;break;}
												case SDLK_BACKSPACE:{break;}
												case SDLK_s:{ save(); break;}
												default:break;
										}///<switch (event.key.keysym.sym)
										break;
								}///<case SDL_KEYDOWN:
								case SDL_MOUSEBUTTONDOWN:{break;}

								default:break;
	
						}///<switch (event.type)
				}///while SDL_PollEvent(&event)
	
				if(cnt<2){
						cnt++;
						plotMap();
						
						for(unsigned int i=0; i<result.storage.size();i++){
								Grid::TPixel top=grid->toPixel(Grid::TCoord2D(result.storage[i].x-result.storage[i].ax,
																							 result.storage[i].y-result.storage[i].ay));
								Grid::TPixel bot=grid->toPixel(Grid::TCoord2D(result.storage[i].x+result.storage[i].ax,
																							 result.storage[i].y+result.storage[i].ay));
								mapToScreenPixel(top);mapToScreenPixel(bot);
								float w = result.storage[i].weight;
								float wval = exp(-(w*w)/(variance*variance));
								boxRGBA(screen, top.x, top.y, bot.x,bot.y, 0, (int)(15+240*wval), 0, (int)(5+250*wval));
								
								top = grid->toPixel(Grid::TCoord2D(result.storage[i].x,result.storage[i].y));
								mapToScreenPixel(top);
								float ang = a-result.storage[i].aa;
								bot = grid->toPixel(Grid::TCoord2D(result.storage[i].x+2.5*cos(ang),result.storage[i].y+2.5*sin(ang)));
								mapToScreenPixel(bot);
								lineRGBA(screen, top.x , top.y, bot.x , bot.y, 0, 15+240*wval, 100,(int) (5+250*wval));
								ang = a+result.storage[i].aa;
								bot = grid->toPixel(Grid::TCoord2D(result.storage[i].x+2.5*cos(ang),result.storage[i].y+2.5*sin(ang)));
								mapToScreenPixel(bot);
								lineRGBA(screen, top.x , top.y, bot.x , bot.y, 0, 15+240*wval, 100, (int) (5+250*wval));
						}
						///The best estimate
						Grid::TPixel pix=grid->toPixel(Grid::TCoord2D(result.storage[0].x,result.storage[0].y));
						mapToScreenPixel(pix);
						circleRGBA(screen, pix.x, pix.y,10, 0, 255, 0, 255);
						Grid::TPixel pix2=grid->toPixel(Grid::TCoord2D(result.storage[0].x+2.5*cos(a),
																														result.storage[0].y+2.5*sin(a)));
						mapToScreenPixel(pix2);
						lineRGBA(screen, pix.x , pix.y, pix2.x , pix2.y, 0, 255, 0,255);
						
						///The true pose
						pix=grid->toPixel(Grid::TCoord2D(x,y));
						mapToScreenPixel(pix);
						circleRGBA(screen, pix.x, pix.y,5, 255, 0, 0, 255);
						pix2=grid->toPixel(Grid::TCoord2D(x+2.5*cos(a),y+2.5*sin(a)));
						mapToScreenPixel(pix2);
						lineRGBA(screen, pix.x , pix.y, pix2.x , pix2.y, 255, 255, 0,255);

						printGeneralInfo();
				}
				
				
		
				SDL_Flip(screen);
				usleep(1000*10);
		//fprintf(stderr,"Running\n");
		}///<While(run)
	
		cleanSDL();
		SDL_Quit();
		return out;
}

///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
/// Plot result
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////

bool COccuGUI::plotMatchResultsWithPoints(CMatchStorage &result, CEnvironmentMeasurement2D &meas, 
																														float x, float y, float a,float variance){
		SDL_Event event;
		bool run = true;	
		initSDL();
		int Nx,Ny;
		grid->getDim(Nx,Ny);
		float *data;
		data = grid->getDataPtr();
		int cnt = 0;
		bool out = true;
		while(run){
		// Wait for incoming event
				while (SDL_PollEvent(&event)) {
			// Switch based on event type.
						switch (event.type) {
								case SDL_QUIT: {run = false;break;}
								case SDL_KEYDOWN: {
										switch (event.key.keysym.sym) {
												case SDLK_UP: break;
												case SDLK_DOWN:break;
												case SDLK_q:{run = false; out = false; break;}
												case SDLK_ESCAPE:{run = false;break;}
												case SDLK_BACKSPACE:{break;}
												case SDLK_s:{ save(); break;}
												default:break;
										}///<switch (event.key.keysym.sym)
										break;
								}///<case SDL_KEYDOWN:
								case SDL_MOUSEBUTTONDOWN:{break;}

								default:break;
	
						}///<switch (event.type)
				}///while SDL_PollEvent(&event)
	
				if(cnt<2){
						cnt++;
						plotMap();
						///Plot all math poses
						for(unsigned int i=0; i<result.storage.size();i++){
								Grid::TPixel top=grid->toPixel(Grid::TCoord2D(result.storage[i].x-result.storage[i].ax,
																							 result.storage[i].y-result.storage[i].ay));
								Grid::TPixel bot=grid->toPixel(Grid::TCoord2D(result.storage[i].x+result.storage[i].ax,
																							 result.storage[i].y+result.storage[i].ay));
								mapToScreenPixel(top);mapToScreenPixel(bot);
								float w = result.storage[i].weight;
								float wval = exp(-(w*w)/(variance*variance));
								boxRGBA(screen, top.x, top.y, bot.x,bot.y, 0, (int)(15+240*wval), 0, (int)(5+250*wval));
								
								top = grid->toPixel(Grid::TCoord2D(result.storage[i].x,result.storage[i].y));
								mapToScreenPixel(top);
								float ang = a-result.storage[i].aa;
								bot = grid->toPixel(Grid::TCoord2D(result.storage[i].x+2.5*cos(ang),result.storage[i].y+2.5*sin(ang)));
								mapToScreenPixel(bot);
								lineRGBA(screen, top.x , top.y, bot.x , bot.y, 0, 15+240*wval, 100,(int) (5+250*wval));
								ang = a+result.storage[i].aa;
								bot = grid->toPixel(Grid::TCoord2D(result.storage[i].x+2.5*cos(ang),result.storage[i].y+2.5*sin(ang)));
								mapToScreenPixel(bot);
								lineRGBA(screen, top.x , top.y, bot.x , bot.y, 0, 15+240*wval, 100, (int) (5+250*wval));
						}
						///Plot the measurement to best matching pose
						float px,py,dl,da;
						for(unsigned int i=0; i<meas.points.size();i++){
								dl = meas.points[i].getDist();
								da = meas.points[i].getAngle();
								
								px = dl * cos(da+result.storage[0].a) + result.storage[0].x;
								py = dl * sin(da+result.storage[0].a) + result.storage[0].y;
								Grid::TPixel pix=grid->toPixel(Grid::TCoord2D(px,py));
								mapToScreenPixel(pix);
								pixelRGBA(screen, pix.x,pix.y,255,255,0,255);
						}
						
						
						///The best estimate
						Grid::TPixel pix=grid->toPixel(Grid::TCoord2D(result.storage[0].x,result.storage[0].y));
						mapToScreenPixel(pix);
						circleRGBA(screen, pix.x, pix.y,10, 0, 255, 0, 255);
						Grid::TPixel pix2=grid->toPixel(Grid::TCoord2D(result.storage[0].x+2.5*cos(result.storage[0].a),
																						result.storage[0].y+2.5*sin(result.storage[0].a)));
						mapToScreenPixel(pix2);
						lineRGBA(screen, pix.x , pix.y, pix2.x , pix2.y, 0, 255, 0,255);
						
						///The true pose
						pix=grid->toPixel(Grid::TCoord2D(x,y));
						mapToScreenPixel(pix);
						circleRGBA(screen, pix.x, pix.y,5, 255, 0, 0, 255);
						pix2=grid->toPixel(Grid::TCoord2D(x+2.5*cos(a),y+2.5*sin(a)));
						mapToScreenPixel(pix2);
						lineRGBA(screen, pix.x , pix.y, pix2.x , pix2.y, 255, 255, 0,255);

						printGeneralInfo();
				}
				
				
		
				SDL_Flip(screen);
				usleep(1000*10);
		//fprintf(stderr,"Running\n");
		}///<While(run)
	
		cleanSDL();
		SDL_Quit();
		return out;
}
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
bool COccuGUI::plotEnvironmentMeasurement2D(CEnvironmentMeasurement2D &meas){
		SDL_Event event;
		bool run = true;	
		initSDL();
		int cnt = 0;
		bool out=true; 
		while(run){
		// Wait for incoming event
				while (SDL_PollEvent(&event)) {
			// Switch based on event type.
						switch (event.type) {
								case SDL_QUIT: {run = false;break;}
								case SDL_KEYDOWN: {
										switch (event.key.keysym.sym) {
												case SDLK_UP: break;
												case SDLK_DOWN:break;
												case SDLK_q:{run = false;out = false;break;}
												case SDLK_ESCAPE:{run = false;  break;}
												case SDLK_BACKSPACE:{cnt=0;break;}
												case SDLK_s:{ save(); break;}
												default:break;
										}///<switch (event.key.keysym.sym)
										break;
								}///<case SDL_KEYDOWN:
								case SDL_MOUSEBUTTONDOWN:{break;}
								default:break;
						}///<switch (event.type)
				}///while SDL_PollEvent(&event)
		
				if(cnt<2){
						cnt++;
						plotMap();
						printGeneralInfo();
				
						///Plot the measurement to best matching pose
						float px,py,dl,da;
						for(unsigned int i=0; i<meas.points.size();i++){
								dl = meas.points[i].getDist();
								da = meas.points[i].getAngle();
												
								px = dl * cos(da+meas.frame.a_ref) + meas.frame.x_ref;
								py = dl * sin(da+meas.frame.a_ref) + meas.frame.y_ref;
								Grid::TPixel pix=grid->toPixel(Grid::TCoord2D(px,py));
								mapToScreenPixel(pix);
								pixelRGBA(screen, pix.x,pix.y,255,255,0,255);
						}
				}
				SDL_Flip(screen);
				usleep(1000*10);
		}
		cleanSDL();
		SDL_Quit();
		return out;
}


///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////

void COccuGUI::save(){
		int Nx,Ny;
		grid->getDim(Nx,Ny);
		initSDL();
		plotMap();
		
		char *rgb=(char *) malloc(3*Nx*Ny);
		
		/* Extracting color components from a 32-bit color value */
		SDL_PixelFormat *fmt;
		Uint32 temp, pixel;
		Uint8 red, green, blue, alpha;
		for(unsigned int i=0; i<Nx*Ny; i++){
				fmt = screen->format;
				SDL_LockSurface(screen);
				pixel = *((Uint32*)(screen->pixels)+i);
				SDL_UnlockSurface(screen);
		
				/* Get Red component */
				temp = pixel & fmt->Rmask;  /* Isolate red component */
				temp = temp >> fmt->Rshift; /* Shift it down to 8-bit */
				temp = temp << fmt->Rloss;  /* Expand to a full 8-bit number */
				red = (Uint8)temp;
		
				/* Get Green component */
				temp = pixel & fmt->Gmask;  /* Isolate green component */
				temp = temp >> fmt->Gshift; /* Shift it down to 8-bit */
				temp = temp << fmt->Gloss;  /* Expand to a full 8-bit number */
				green = (Uint8)temp;
		
				/* Get Blue component */
				temp = pixel & fmt->Bmask;  /* Isolate blue component */
				temp = temp >> fmt->Bshift; /* Shift it down to 8-bit */
				temp = temp << fmt->Bloss;  /* Expand to a full 8-bit number */
				blue = (Uint8)temp;
				
				rgb[3*i] = red;
				rgb[3*i+1] = green;
				rgb[3*i+2] = blue;				
		}
		CJPEGEncoder encoder;
		encoder.SetRGBDataPtr((const char *)rgb,Nx,Ny);
		time_t seconds;
		seconds = time(NULL);
		char fname[100];
		sprintf(fname,"%ld_screen_shot.jpg",seconds);
		//fprintf(stcerr,"Saving a ");
		
		FILE *f = fopen(fname,"wb");
		fwrite(encoder.GetJPEGDataPtr(), 
					 encoder.GetJPEGDataSize(), 
					 1, f);
		fclose(f);

		cleanSDL();
		SDL_Quit();
		free(rgb);
}

///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
bool COccuGUI::initSDL(){
	int Nx,Ny;
	grid->getDim(Nx,Ny);
	
	if(SDL_Init(SDL_INIT_VIDEO) == -1){
		fprintf(stderr,"Failed to initialize SDL Video: %s\n", SDL_GetError());
		exit(1);
	}
  
  // Register cleanup function for SDL
	atexit(SDL_Quit);
  
  // Initialize SDL-Video in mode 800x600, 32bpp, & some flags.
	if ((screen = SDL_SetVideoMode(Nx*scale, Ny*scale, 32, SDL_HWSURFACE|SDL_DOUBLEBUF|SDL_RESIZABLE)) == NULL) {
				 fprintf(stderr, "Unable to set video mode for SDL Context: %s\n", SDL_GetError());
				 abort();
	}
	 // Clear the screen
	SDL_FillRect(screen, NULL, 0);

  // Set imporant info :)
	SDL_WM_SetCaption("OccuGUI - 2008","OccuGUI");
}

bool COccuGUI::cleanSDL(){
	SDL_FreeSurface(screen);
	screen=NULL;
}

void COccuGUI::printGeneralInfo(){
	stringRGBA(screen, 5, 5, "Keys:", 255, 0, 0, 150);
	stringRGBA(screen, 5, 15, "-'Esc' or 'Enter' Ends the dialog", 255, 0, 0, 150);
	stringRGBA(screen, 5, 25, "-'Backspace' to erase last item", 255, 0, 0, 150);
	stringRGBA(screen, 5, 35, "-'S' Capture screen shot", 255, 0, 0, 150);
	stringRGBA(screen, 5, 45, "-'Q' Ends the dialog and Exits the program", 255, 0, 0, 150);
}